#include <memory>
#include <thread>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "arm_control/action/arm.hpp"
#include "arm_control/arm_action_type1.hpp"
#include "arm_control/arm_action_type2.hpp"
#include "arm_control/arm_action_type3.hpp"
#include "arm_control/arm_action_type4.hpp"
#include "arm_control/arm_action_type5.hpp"
#include "arm_control/arm_action_type6.hpp"
#include "arm_control/arm_action_type7.hpp"
#include "arm_control/arm_action_type8.hpp"
#include "arm_control/arm_action_type9.hpp"
#include "arm_control/arm_action_type10.hpp"
#include "arm_control/arm_action_type11.hpp"

class ArmControlServer : public rclcpp::Node
{
public:
  using ARM = arm_control::action::ARM;
  using GoalHandleARM = rclcpp_action::ServerGoalHandle<ARM>;

  ArmControlServer() : Node("arm_control_server")
  {
    RCLCPP_INFO(this->get_logger(), "机械臂控制动作服务器节点已启动");
  }

  void initialize()
  {
    // 创建各个动作类型的实例（动作类型1-11都需要节点指针）
    action_type1_ = std::make_unique<arm_control::ArmActionType1>(this->get_logger(), Node::shared_from_this());
    action_type2_ = std::make_unique<arm_control::ArmActionType2>(this->get_logger(), Node::shared_from_this());
    action_type3_ = std::make_unique<arm_control::ArmActionType3>(this->get_logger(), Node::shared_from_this());
    action_type4_ = std::make_unique<arm_control::ArmActionType4>(this->get_logger(), Node::shared_from_this());
    action_type5_ = std::make_unique<arm_control::ArmActionType5>(this->get_logger(), Node::shared_from_this());
    action_type6_ = std::make_unique<arm_control::ArmActionType6>(this->get_logger(), Node::shared_from_this());
    action_type7_ = std::make_unique<arm_control::ArmActionType7>(this->get_logger(), Node::shared_from_this());
    action_type8_ = std::make_unique<arm_control::ArmActionType8>(this->get_logger(), Node::shared_from_this());
    action_type9_ = std::make_unique<arm_control::ArmActionType9>(this->get_logger(), Node::shared_from_this());
    action_type10_ = std::make_unique<arm_control::ArmActionType10>(this->get_logger(), Node::shared_from_this());
    action_type11_ = std::make_unique<arm_control::ArmActionType11>(this->get_logger(), Node::shared_from_this());
    
    // 配置action server选项以增加超时时间
    rcl_action_server_options_t server_options = rcl_action_server_get_default_options();
    server_options.result_timeout.nanoseconds = RCL_S_TO_NS(300);  // 结果超时时间300秒
    server_options.goal_service_qos = rmw_qos_profile_services_default;
    server_options.result_service_qos = rmw_qos_profile_services_default;
    
    // 创建独立的回调组以避免阻塞
    callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
    
    // 创建动作服务器
    this->action_server_ = rclcpp_action::create_server<ARM>(
      this,
      "/ARM",
      std::bind(&ArmControlServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
      std::bind(&ArmControlServer::handle_cancel, this, std::placeholders::_1),
      std::bind(&ArmControlServer::handle_accepted, this, std::placeholders::_1),
      server_options,
      callback_group_);
    
    RCLCPP_INFO(this->get_logger(), "动作服务器 '/ARM' 已创建（结果超时: 300秒，使用可重入回调组）");
    RCLCPP_INFO(this->get_logger(), "支持的动作类型: 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11");
  }

private:
  rclcpp_action::Server<ARM>::SharedPtr action_server_;
  rclcpp::CallbackGroup::SharedPtr callback_group_;
  std::unique_ptr<arm_control::ArmActionType1> action_type1_;
  std::unique_ptr<arm_control::ArmActionType2> action_type2_;
  std::unique_ptr<arm_control::ArmActionType3> action_type3_;
  std::unique_ptr<arm_control::ArmActionType4> action_type4_;
  std::unique_ptr<arm_control::ArmActionType5> action_type5_;
  std::unique_ptr<arm_control::ArmActionType6> action_type6_;
  std::unique_ptr<arm_control::ArmActionType7> action_type7_;
  std::unique_ptr<arm_control::ArmActionType8> action_type8_;
  std::unique_ptr<arm_control::ArmActionType9> action_type9_;
  std::unique_ptr<arm_control::ArmActionType10> action_type10_;
  std::unique_ptr<arm_control::ArmActionType11> action_type11_;

  // 处理目标请求
  rclcpp_action::GoalResponse handle_goal(
    const rclcpp_action::GoalUUID & uuid,
    std::shared_ptr<const ARM::Goal> goal)
  {
    (void)uuid;
    RCLCPP_INFO(this->get_logger(), "收到动作请求，类型: %d", goal->action_type);
    
    // 检查动作类型是否有效
    if (goal->action_type < 1 || goal->action_type > 11) {
      RCLCPP_WARN(this->get_logger(), "无效的动作类型: %d (必须是1-11)", goal->action_type);
      return rclcpp_action::GoalResponse::REJECT;
    }
    
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

  // 处理取消请求
  rclcpp_action::CancelResponse handle_cancel(
    const std::shared_ptr<GoalHandleARM> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "收到取消动作请求");
    (void)goal_handle;
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  // 处理已接受的目标
  void handle_accepted(const std::shared_ptr<GoalHandleARM> goal_handle)
  {
    // 在新线程中执行，避免阻塞
    std::thread{std::bind(&ArmControlServer::execute, this, std::placeholders::_1), goal_handle}.detach();
  }

  // 执行动作
  void execute(const std::shared_ptr<GoalHandleARM> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "开始执行动作...");
    
    const auto goal = goal_handle->get_goal();
    auto feedback = std::make_shared<ARM::Feedback>();
    auto result = std::make_shared<ARM::Result>();
    
    // 根据不同的动作类型调用对应的实现
    switch (goal->action_type) {
      case 1:
        action_type1_->execute(goal_handle, feedback, result);
        break;
      case 2:
        action_type2_->execute(goal_handle, feedback, result);
        break;
      case 3:
        action_type3_->execute(goal_handle, feedback, result);
        break;
      case 4:
        action_type4_->execute(goal_handle, feedback, result);
        break;
      case 5:
        action_type5_->execute(goal_handle, feedback, result);
        break;
      case 6:
        action_type6_->execute(goal_handle, feedback, result);
        break;
      case 7:
        action_type7_->execute(goal_handle, feedback, result);
        break;
      case 8:
        action_type8_->execute(goal_handle, feedback, result);
        break;
      case 9:
        action_type9_->execute(goal_handle, feedback, result);
        break;
      case 10:
        action_type10_->execute(goal_handle, feedback, result);
        break;
      case 11:
        action_type11_->execute(goal_handle, feedback, result);
        break;
      default:
        result->success = false;
        result->message = "未知的动作类型";
        goal_handle->abort(result);
        return;
    }
  }
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<ArmControlServer>();
  node->initialize();  // 初始化动作类型和服务器
  
  // 使用多线程执行器以支持可重入回调组
  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(node);
  executor.spin();
  
  rclcpp::shutdown();
  return 0;
}
